#ifndef __TASK_LOGIC__
#define __TASK_LOGIC__
#include <vector>
#include <string>
#include <string.h>
#include <unordered_map>
#include <iostream>

#include <sys/types.h>   
#include <sys/socket.h>   
#include <netinet/in.h>   
#include <netdb.h>   
#include <arpa/inet.h>   
#include <signal.h>   
#include <sys/ioctl.h>   
#include <linux/if.h> 
#include <sys/prctl.h>
//#include <cJSON.h>

#include <boost/property_tree/ptree.hpp>
#include <json_parser_fix.hpp>
#include <shm.h>
//#include <boost/property_tree/json_parser.hpp>

#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Path.h>
#include <ros/ros.h>

#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

#include <thread>
#include <mutex>
#include <wchar.h>

#include "common_msg/Console.h"
#include <std_msgs/String.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/Odometry.h>
#include <common_msg/RunStatus.h>
#include <common_msg/NaviStatus.h>
#include <common_msg/DriveInfo.h>
#include <common_msg/DetectedObjects.h>

#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>

using namespace std;
using namespace boost::property_tree;

enum RobotStatus {IDLE,INIITIALIZED,RUNNING,FINISHED,PAUSED,CANCELLED};
enum ControlMode {AUTO,MANUAL};
enum MappingStatus {FINISHED_MAPPING,RUNNING_MAPPING,FAILED_MAPPING};
enum MappingMode {PRECISE,FAST};
struct Point3D
{
    int gridX;
    int gridY;
    int gridZ;
};

struct MapInfo{
    int gridHeight;
    int gridWidth;
    double resolution;
    double originX;
    double originY;
};
struct MapParam
{
    string mapName;
    string createdAt;
    struct MapInfo mapInfo;
};
struct InitParam
{
    string initPointName;
    bool initDirectly;
};
struct ChgParam
{
    string chgPointName;
    string chgPathName;
    string chgMode;
    int batLowTh;
    int batHighTh;
    string chgStartTime;
    string chgEndTime;
};
struct HeadParam
{
    string headMode;
    string headSpeed;
    int headAngle;
};
struct MotionParam
{
    string naviSpeed;
    string ctrlSpeed;
};
struct Param
{
    string mapName;
    InitParam initParam;
    ChgParam chgParam;
    HeadParam headParam;
    MotionParam motionParam;
};

struct Point
{
    string mapName;
    string pointName;
    int gridX;
    int gridY;
    int angle;
};

struct Path
{
    string mapName;
    string pathName;
    vector<string> point;
};

struct Task
{
    string mapName;
    string name;
    string mode;
    string param;
    string start;
    string end;
    bool run;
    vector<int> repeat;
    int loopCount;
};

struct DefaultTask
{
    string name;
    string mode;
    string param;
};
struct MoveCmd
{
    double linearSpeed;
    double angularSpeed;
};

struct Destination
{
    int gridX;
    int gridY;
    int angle;
};
struct AlongPath
{
    string mapName;
    string pathName;
};

struct ErrorCode
{
    int motorCodeL;
    int motorCodeR;
    int chgSendCode;
    int chgRecvCode;
    int motorCodeD;
};
struct Position
{
    int gridX;
    int gridY;
    int angle;
};
struct Status
{
    struct Task activeTask;
    struct Position position;
    string runState;
    double temperature;
    double battery;
    double batteryVoltage;
    string chargeStatus;
    double chargeCurrent;
    double outputCurrent;
    string motorStatus;
    string emergencyStop;
    string chargePanel;
    double linearSpeed;
    double angularSpeed;
    struct ErrorCode errorCode;
    int uptime;
};

struct GpsData
{
    double altitude;    // 高度(m)
    double latitude;    // 纬度，北正南负
    double longitude;   // 经度，东正西负
    int status;         // -1为没有GPS信号，0为非rtk单点解，1为rtk浮点解，2为rtk固定解
};

struct AlarmStatus
{
    string code;
    string info;
    string time;
};

struct Alarm
{
    vector<AlarmStatus> status;
    vector<AlarmStatus> navigation;
    vector<AlarmStatus> program;
};

struct BasicInfo
{
    int battery;
    bool charge;
    RobotStatus status;
};

class CloudApi
{
    public:
        CloudApi(ros::NodeHandle &nh);
        ~CloudApi();
        void init_data();
        void polling();

        void set_param(Param param);
        Param get_param();
        Param get_param(string mapName);

        void add_navi_point(Point naviPoint);
        void del_navi_point(Point naviPoint);
        void rename_navi_point(Point naviPoint);

        void add_navi_path(Path naviPath);
        void del_navi_path(string mapName, string pathName);

        void add_task(Task task, string mapName, string taskName);
        void del_task(string mapName, string taskName);
        Task get_default_task();
        void set_default_task(Task task);
        void del_default_task();
        bool edit_map(string map_name);
        string load_map(string mapName);
        ptree get_load_map_status();

        void set_navi_speed(int level);
        void initialize_localizaition(string mapName, string point_name);
        void initialize_localizaition(string mapName, int pos[2], int angle);
        void initialize_localizaition(string mapName, double init_pos[3], double init_ori[4]);
        void initialize_localizaition();
        ptree get_initialize_status();
        string get_navi_point();
        string get_navi_path();
//        void get_local_map();
        void update_navi_info();
        string get_task();
        void start_task(char buf[]);
        void stop_task();
        ptree get_task_status();
        void set_move_cmd(char buf[]);
        void navi_to_point(char buf[]);
        void navi_to_dest(char buf[]);
        void navi_along_path(char buf[]);

        Status get_status();
        GpsData get_gps_data();
        Alarm get_alarm();

        void driver_thread();
        void localization_thread();
        void navigation_thread();
        void scan_thread();
        void mapping_thread();

        ptree get_basic_info();
        string get_map_list();

        void set_control_mode(string mode);

        bool download_file(char buf[]);
        ptree get_3d_lidar();
        ptree get_2d_lidar();
        ptree get_camera();
        ptree get_sonar();
        ptree get_infra();
        ptree get_key();
        ptree get_gps();
        ptree get_odom();
        ptree get_imu();
        ptree get_rfid();
        ptree get_uwb();

        void set_safe_area(double r1,double r2, double r3);
        bool set_init_point(char buf[]);
        bool set_calibration_point(char buf[]);
        bool set_work_point(char buf[]);
        bool set_charge_point(char buf[]);
        bool set_navi_point(char buf[]);
        bool set_user_point(char buf[]);
        bool set_navi_path(char buf[]);
        bool set_virtual_wall(char buf[]);
        bool set_virtual_wall_point(char buf[]);
        bool set_user_path(char buf[]);
        bool set_user_path_point(char buf[]);
        bool set_slow_area(char buf[]);
        bool set_slow_area_point(char buf[]);
        bool set_stop_area(char buf[]);
        bool set_stop_area_point(char buf[]);
        bool set_user_area(char buf[]);
        bool set_user_area_point(char buf[]);

        string get_file_string(string uri);

        string get_init_point(string map_name);
        string get_calibration_point(string map_name);
        string get_navi_point(string map_name);
        string get_work_point(string map_name);
        string get_charge_point(string map_name);
        string get_user_point(string map_name);

        string get_navi_path(string map_name);
        string get_virtual_wall(string map_name);
        string get_user_path(string map_name);
        string get_virtual_wall_point(string map_name);
        string get_user_path_point(string map_name);

        string get_slow_area(string map_name);
        string get_stop_area(string map_name);
        string get_user_area(string map_name);

        string get_slow_area_point(string map_name);
        string get_stop_area_point(string map_name);
        string get_user_area_point(string map_name);

        bool set_mapping_mode(string map_name, string mode);
        void start_scan();
        void cancel_scan();
        void finish_scan();
        ptree get_scan_status();
        void start_mapping();
        void cancel_mapping();
        void get_mapping_status(string &msg);
        void global_position();
        void initial_position(char buf[]);
        void start_route(char buf[]);
        ptree get_running_status();
        void pause_task();
        void resume_task();

        bool is_map_exist(string map_name);
        bool del_map(string map_name);

    private:

        void start_localization_navigation();

        bool get_navi_from_point(string pointName,geometry_msgs::PoseStamped &pose);

        bool params_speed(int level);

        bool params_localization(string map_name, geometry_msgs::Pose pose);

        bool params_map_name(string map_name);

        int angle_navi_to_api(double angle);

        double angle_api_to_navi(int angle);

        void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy);

        bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my);

        bool getRobotPose(tf::Stamped<tf::Pose>& global_pose);

        void scanStatusCallback(const std_msgs::String::ConstPtr& scan_status);
        
        void mappingStatusCallback(const std_msgs::String::ConstPtr& mapping_status);

        void killStatusCallback(const std_msgs::String::ConstPtr& msg);

        void naviStatusCallback(const common_msg::NaviStatus::ConstPtr& navi_status);

        void laser3DCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
        void laser2DCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
        void cameraCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
        void driveCallback(const common_msg::DriveInfo::ConstPtr& msg);
        void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);

        void objectCallback(const common_msg::DetectedObjects::ConstPtr& msg);
       
        bool write_file(char buf[], string file_name);
        
        int rm_dir(std::string dir_full_path);
        
        void remove_map(string map_name);

        void transform_point(int pos[2], int angle, geometry_msgs::Pose &pose);
        void transform_point(string point_name, geometry_msgs::Pose &pose);
        void publish_init_position(geometry_msgs::Pose pose);
        void publish_goal(geometry_msgs::Pose goal);

        void publish_parking_point();

        double origin_x_,origin_y_,resolution_;
        unsigned int size_x_,size_y_;

        BasicInfo basic_info_;

        RobotStatus robot_status_;
        bool position_ok;
        DefaultTask default_task_;
        Task current_task_;
        string current_map_;
        string current_init_point_;
        string config_file_path_;

        vector<Point> navi_point_;
        vector<Path> navi_path_;
        vector<Task> task_;
        vector<MapParam> map_list_;
        Destination current_pos_;

        nav_msgs::Path point_queue;
        int current_num{0};
        int total_point_num{0};
        std::mutex mutex_1;

        ros::Publisher console_pub_;
        ros::Publisher cmd_pub_;
        ros::Publisher path_pub_;
        ros::Publisher parking_pub_;
        ros::Publisher goal_op_pub_;
        ros::Publisher run_status_pub_;
        ros::Publisher scan_pub_;
        ros::Publisher mapping_pub_;
        ros::Publisher initpos_pub_;
        ros::Publisher goal_pub_;       

        ros::Subscriber scan_sub_;
        ros::Subscriber mapping_sub_;
        ros::Subscriber navi_sub_;
        ros::Subscriber process_sub_;
        ros::Subscriber laser_3d_sub_;
        ros::Subscriber laser_2d_sub_;
        ros::Subscriber camera_sub_;
        ros::Subscriber drive_sub_; // sonar, gps, infra, rfid, uwb_sub
        ros::Subscriber odom_sub_;
        ros::Subscriber object_sub_;

        ros::NodeHandle nh_;
        actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> *ac_;
        
        vector<Point3D> lidar_3d_data, lidar_3d_data_lastest;
        vector<Point3D> lidar_2d_data, lidar_2d_data_lastest;
        vector<Point3D> camera_data, camera_data_lastest;
        MoveCmd current_speed_;
        GpsData current_gps_;
        double roll_,pitch_,yaw_;
        double sonar_data_[12];
        bool infra_data_[3];
        bool key_data_[3];
        double safe_area_[3];
        std::mutex mutex_3d_lidar;
        std::mutex mutex_2d_lidar;
        std::mutex mutex_camera;

        ControlMode current_control_mode_;
        MappingStatus mapping_status_;
        MappingMode mapping_mode_;

        bool map_loaded_{false};
        string map_load_status_{"finished"};
        string scan_status_{"finished"};
        Shm set_speed_shm;
        Shm set_map_name_shm;

        bool parking_flag_{false};
        geometry_msgs::PoseStamped parking_pose;
        double dis_to_park_;
};
#endif